// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ImageRectangle.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Vision.ImageProcessing2D
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Extracted rectangle from an image
    /// </summary>
    public class ImageRectangle : IShape2D
    {
        /// <summary>
        /// Edges of the rectangle in increasing order of length, so first 2 are the short edges
        /// last 2 are the major length edges
        /// </summary>
        public readonly List<MergedLines> Edges;

        /// <summary>
        /// Four rectangle corners
        /// </summary>
        public readonly List<Point2D<int>> Corners;

        /// <summary>
        /// Angle of major side with image x axis (columns) in degree
        /// </summary>
        public readonly double MajorSideAngle;

        /// <summary>
        /// Angle of minor side with image X axis (columns) in degree
        /// </summary>
        public readonly double MinorSideAngle;

        /// <summary>
        /// Tracing context for image rectangles
        /// </summary>
        private static TraceContext rectTraceContext = TraceContexts.FromString(TraceContexts.Vision, "ImageRect");

        /// <summary>
        /// Initializes a new instance of the <see cref="ImageRectangle" /> class
        /// </summary>
        /// <param name="edges">Rectangle edges</param>
        public ImageRectangle(IEnumerable<MergedLines> edges)
        {
            this.Edges = new List<MergedLines>(edges);
            if (this.Edges.Count != 4)
            {
                throw new ArgumentException(string.Format("Rectangles expect 4 edges found {0}", this.Edges.Count));
            }

            this.Edges.Sort((a, b) => a.TotalLength.CompareTo(b.TotalLength));

            this.CenterX = 0;
            this.CenterY = 0;
            this.Confidence = 0;
            foreach (MergedLines l in this.Edges)
            {
                this.CenterX += l.Start.X + l.End.X;
                this.CenterY += l.Start.Y + l.End.Y;
                this.Confidence += l.AverageEdgeStrength;
            }

            this.CenterX /= this.Edges.Count * 2.0;
            this.CenterY /= this.Edges.Count * 2.0;
            this.Confidence /= this.Edges.Count;

            this.MinorSideAngle = ImageRectangle.AverageAngleWithReflection(this.Edges[0].Angle, this.Edges[1].Angle);
            this.MajorSideAngle = ImageRectangle.AverageAngleWithReflection(this.Edges[2].Angle, this.Edges[3].Angle);
        }

        /// <summary>
        /// Gets major edge length
        /// </summary>
        public double MajorLength
        {
            get
            {
                return (this.Edges[2].TotalLength + this.Edges[3].TotalLength) / 2.0;
            }
        }

        /// <summary>
        /// Gets minor edge length
        /// </summary>
        public double MinorLength
        {
            get
            {
                return (this.Edges[0].TotalLength + this.Edges[1].TotalLength) / 2.0;
            }
        }

        /// <summary>
        /// Gets center of rectangle - X
        /// </summary>
        public double CenterX { get; private set; }

        /// <summary>
        /// Gets center of rectangle Y
        /// </summary>
        public double CenterY { get; private set; }

        /// <summary>
        /// Gets confidence for rectangle
        /// </summary>
        public double Confidence { get; private set; }

        /// <summary>
        /// Gets size as half the diagonal of the rectangle
        /// </summary>
        public double Size1D
        {
            get
            {
                double l1 = this.MajorLength / 2.0;
                double l2 = this.MinorLength / 2.0;
                return Math.Sqrt(l1 * l1 + l2 * l2);
            }
        }

        /// <summary>
        /// Finds rectangles from a collection of lines extracted from an image. The algorithm is brute force
        /// 1) Treat the passed in lines as line segments, merge into longer lines
        /// 2) Greedily search through the merged lines for pairs of rectangle side candidates (parallel lines)
        /// 3) For each pair look for matching pairs (parallel of length matching separation to the candidate)
        /// 4) Finally check that end points of set identified in (3) intersect
        /// </summary>
        /// <param name="lines">candidate lines</param>
        /// <param name="angleMergeThreshold">Merge angle threshold (degree)</param>
        /// <param name="normMergeThreshold">Merge line distance from origin norm threshold (pixel)</param>
        /// <param name="endPointMergeThreshold">End point merge threshold (fractional of line length)</param>
        /// <param name="minSideLength">Minimum side length</param>
        /// <returns>List of rectangles found</returns>
        public static List<ImageRectangle> FindRectangles(
            List<ImageLine> lines, 
            double angleMergeThreshold, 
            double normMergeThreshold, 
            double endPointMergeThreshold, 
            double minSideLength)
        {
            if (endPointMergeThreshold < 0.0 || endPointMergeThreshold > 1.0)
            {
                throw new ArgumentException(string.Format("EndPointMergeThreshold must be in range [0:1] got value {0}", endPointMergeThreshold));
            }

            List<ImageRectangle> foundRectangles = new List<ImageRectangle>();
            if (lines.Count <= 2)
            {
                return foundRectangles;
            }

            int maxBinAngle = (int)((Math.PI / MathConstants.Degrees2Radians / angleMergeThreshold) + 0.5);

            List<MergedLines> mergedLines = MergedLines.DoLineMerge(lines, angleMergeThreshold, normMergeThreshold, endPointMergeThreshold, minSideLength, maxBinAngle);

            // sort lines by (major side) angle
            mergedLines.Sort(
                (a, b) =>
                {
                    double angA = Math.Min(a.BinnedAngle, a.BinnedAngleRoundUp);
                    double angB = Math.Min(b.BinnedAngle, b.BinnedAngleRoundUp);
                    if (angA == angB)
                    {
                        return a.TotalLength.CompareTo(b.TotalLength);
                    }

                    return angA.CompareTo(angB);
                });

            ImageRectangle rect = null;
            for (int i = 0; i < mergedLines.Count; ++i)
            {
                MergedLines mii = mergedLines[i];
                double maxLength = mii.TotalLength * (1.0 + endPointMergeThreshold);

                for (int j = i + 1; j < mergedLines.Count; ++j)
                {
                    MergedLines mij = mergedLines[j];
                    if (Math.Min(mij.BinnedAngle, mij.BinnedAngleRoundUp) != Math.Min(mii.BinnedAngle, mii.BinnedAngleRoundUp) || mij.TotalLength > maxLength)
                    {
                        break;
                    }

                    // Lines are parallel proceed to next level of checks
                    if (true == ImageRectangle.IsRectanglePart2(mergedLines, mii, mij, endPointMergeThreshold, ref rect))
                    {
                        foundRectangles.Add(rect);
                    }
                }
            }

            // Remove near dups
            ImageRectangle.RemoveDuplicates(foundRectangles, 0.02);

            return foundRectangles;
        }

        /// <summary>
        /// Remove duplicate rectangles from a list
        /// </summary>
        /// <param name="rectangles">List of rectangles</param>
        /// <param name="tolerance">Removal tolerance</param>
        public static void RemoveDuplicates(List<ImageRectangle> rectangles, double tolerance)
        {
            for (int i = 0; i < rectangles.Count; ++i)
            {
                for (int j = i + 1; j < rectangles.Count; ++j)
                {
                    if (true == ImageRectangle.IsDuplicate(rectangles[i], rectangles[j], tolerance))
                    {
                        rectangles.RemoveAt(j);
                        --j;
                    }
                }
            }
        }

        /// <summary>
        /// Check if two rectangles are identical to within a tolerance factor, by looking at the difference in side lengths
        /// and rotation angle
        /// </summary>
        /// <param name="rect1">First rectangle</param>
        /// <param name="rect2">Second Rectangle</param>
        /// <param name="toleranceFactor">Tolerance factor</param>
        /// <returns>True if rectangles are duplicates</returns>
        public static bool IsDuplicate(ImageRectangle rect1, ImageRectangle rect2, double toleranceFactor)
        {
            bool isDup = false;
            double tol2 = Math.Sqrt(toleranceFactor);

            // Step 1 If the  relative difference between the major, minor length and rotaion angles
            // are within the tolerance factor then rectangles are candidate dups
            if ((Math.Abs(rect1.MajorLength - rect2.MajorLength) < rect1.MajorLength * tol2) &&
                 (Math.Abs(rect1.MinorLength - rect2.MinorLength) < rect1.MinorLength * tol2) &&
                (Math.Abs(rect1.MajorSideAngle - rect2.MajorSideAngle) < rect1.MajorSideAngle * tol2))
            {
                // Finally check if rectangle centre distance from origin are within a tolerance factor
                double d = MergedLines.LineEndDistance(rect1.CenterX, rect1.CenterY, rect2.CenterX, rect2.CenterY);
                double d1 = MergedLines.LineEndDistance(rect1.CenterX, rect1.CenterY, 0, 0);
                isDup = d < d1 * toleranceFactor;
            }

            return isDup;
        }

        /// <summary>
        /// Returns an ordered set of rectangle corners, by moving clockwise or counter clockwise around rectangles
        /// </summary>
        /// <returns>Set of corners</returns>
        public List<Point2D<int>> GetCorners()
        {
            List<Point2D<int>> corners = new List<Point2D<int>>();

            Point2D<double> corner = this.Edges[0].Start;
            Point2D<double> close = ImageRectangle.GetClosestPoint(corner, new List<MergedLines>() { this.Edges[2], this.Edges[3] });
            corners.Add(new Point2D<int>((int)((corner.X + close.X) / 2), (int)((corner.Y + close.Y) / 2)));

            corner = this.Edges[0].End;
            close = ImageRectangle.GetClosestPoint(corner, new List<MergedLines>() { this.Edges[2], this.Edges[3] });
            corners.Add(new Point2D<int>((int)((corner.X + close.X) / 2), (int)((corner.Y + close.Y) / 2)));

            corner = ImageRectangle.GetClosestPoint(corner, new List<MergedLines>() { this.Edges[1] });
            close = ImageRectangle.GetClosestPoint(corner, new List<MergedLines>() { this.Edges[2], this.Edges[3] });
            corners.Add(new Point2D<int>((int)((corner.X + close.X) / 2), (int)((corner.Y + close.Y) / 2)));

            Point2D<double> start = this.Edges[1].Start;
            Point2D<double> end = this.Edges[1].End;
            corner = (MergedLines.LineEndDistance(corner.X, corner.Y, end.X, end.Y) <= MathConstants.ErrorEpsilon) ? start : end;
            close = ImageRectangle.GetClosestPoint(corner, new List<MergedLines>() { this.Edges[2], this.Edges[3] });
            corners.Add(new Point2D<int>((int)((corner.X + close.X) / 2), (int)((corner.Y + close.Y) / 2)));

            return corners;
        }

        /// <summary>
        /// Checks if 2 lines parallel are aligned, that is if they can be opposite sides of a rectangle
        /// Note this routine does not check the requirement that the lines are indeed parallel
        /// </summary>
        /// <param name="start1">Line 1 start</param>
        /// <param name="end1">Line 1 end</param>
        /// <param name="start2">Line 2 start</param>
        /// <param name="end2">Line 2 end</param>
        /// <param name="distanceBetween">Expected distance between lines</param>
        /// <param name="maxDeviation">Maximum deviation allowed</param>
        /// <returns>True if criteria are met</returns>
        private static bool IsAligned(Point2D<double> start1, Point2D<double> end1, Point2D<double> start2, Point2D<double> end2, double distanceBetween, double maxDeviation)
        {
            bool isAligned = false;
            double dist1 = MergedLines.LineEndDistance(start1.X, start1.Y, start2.X, start2.Y);

            if (Math.Abs(dist1 - distanceBetween) < maxDeviation)
            {
                double dist2 = MergedLines.LineEndDistance(end1.X, end1.Y, end2.X, end2.Y);
                isAligned = Math.Abs(dist1 - distanceBetween) < maxDeviation;
            }
            else
            {
                dist1 = MergedLines.LineEndDistance(start1.X, start1.Y, end2.X, end2.Y);
                if (Math.Abs(dist1 - distanceBetween) < maxDeviation)
                {
                    double dist2 = MergedLines.LineEndDistance(end1.X, end1.Y, start2.X, start2.Y);
                    isAligned = Math.Abs(dist1 - distanceBetween) < maxDeviation;
                }
            }

            return isAligned;
        }

        /// <summary>
        /// Perform part 2 check for a rectangle. That is given 2 candidate opposite side of a rectangle, 
        /// search for 2 more lines from the input set that can complete a rectangle
        /// </summary>
        /// <param name="mergedLines">Input set of lines</param>
        /// <param name="side1">Candidate side 1</param>
        /// <param name="side2">Candidate side 2</param>
        /// <param name="distanceThreshold">Distance threshold</param>
        /// <param name="rect">Found rectangle</param>
        /// <returns>True if a rectangle is found</returns>
        private static bool IsRectanglePart2(List<MergedLines> mergedLines, MergedLines side1, MergedLines side2, double distanceThreshold, ref ImageRectangle rect)
        {
            bool found = false;
            if (mergedLines.Count < 2)
            {
                return false;
            }

            double distBetween = MergedLines.LineEndDistance((side1.Start.X + side1.End.X) / 2.0, (side1.Start.Y + side1.End.Y) / 2.0, (side2.Start.X + side2.End.X) / 2.0, (side2.Start.Y + side2.End.Y) / 2.0); 
            double maxDistance = side1.TotalLength * distanceThreshold;

            if (true == ImageRectangle.IsAligned(side1.Start, side1.End, side2.Start, side2.End, distBetween, maxDistance))
            {
                found = true;
            }
            else
            {
                found = ImageRectangle.IsAligned(side1.Start, side1.End, side2.End, side2.Start, distBetween, maxDistance);
            }

            if (true == found)
            {
                found = ImageRectangle.FindMatchingSides(mergedLines, side1, side2, 0.0, distBetween, maxDistance, ref rect);

                // Try 2 overlapping bin rounding strategies
                if (false == found)
                {
                    found = ImageRectangle.FindMatchingSides(mergedLines, side1, side2, 0.5, distBetween, maxDistance, ref rect);
                }
            }

            return found;
        }

        /// <summary>
        /// Attempt to find a matching side to a rectangle given to parallel sides. To satisfy the match
        /// candidates must satisfy:
        /// a) Length matches distance between input sides
        /// b) Lines are orthogonal to input parallel lines
        /// c) End points intersect the input lines
        /// </summary>
        /// <param name="mergedLines">Input set of lines to search for matching pair. Set must be sorted by line angle</param>
        /// <param name="side1">First parallel side</param>
        /// <param name="side2">Second Parallel side</param>
        /// <param name="angleBinFactor">Angle binning lookup rounding factor</param>
        /// <param name="distBetween">Distance between passed in sides</param>
        /// <param name="distanceThreshold">Threshold for comparing distances</param>
        /// <param name="rect">Found rectangle</param>
        /// <returns>True if a match is found, false otherwise</returns>
        private static bool FindMatchingSides(List<MergedLines> mergedLines, MergedLines side1, MergedLines side2, double angleBinFactor, double distBetween, double distanceThreshold, ref ImageRectangle rect)
        {
            bool found = false;
            int targedBin = MergedLines.FloatAngleToBin((((side1.Angle + side2.Angle) / 2.0) + 90) % 180, angleBinFactor);

            for (int i = 0; i < mergedLines.Count; ++i)
            {
                MergedLines other1 = mergedLines[i];
                if (other1.BinnedAngle > targedBin && other1.BinnedAngleRoundUp > targedBin)
                {
                    break;
                }

                if (other1.BinnedAngle == targedBin || other1.BinnedAngleRoundUp == targedBin)
                {
                    if (Math.Abs(distBetween - other1.TotalLength) < distanceThreshold &&
                        true == side1.DoLinesIntersect(other1, distanceThreshold))
                    {
                        for (int j = 0; j < mergedLines.Count; ++j)
                        {
                            if (i == j)
                            {
                                continue;
                            }

                            MergedLines other2 = mergedLines[j];
                            if (other2.BinnedAngle > targedBin && other2.BinnedAngleRoundUp > targedBin)
                            {
                                break;
                            }

                            if (other2.BinnedAngle == targedBin || other2.BinnedAngleRoundUp == targedBin)
                            {
                                if (Math.Abs(distBetween - other2.TotalLength) < distanceThreshold &&
                                     true == side1.DoLinesIntersect(other2, distanceThreshold))
                                {
                                    double distBetween2 = MergedLines.LineEndDistance((other1.Start.X + other1.End.X) / 2.0, (other1.Start.Y + other1.End.Y) / 2.0, (other2.Start.X + other2.End.X) / 2.0, (other2.Start.Y + other2.End.Y) / 2.0);

                                    if (Math.Abs(distBetween2 - side1.TotalLength) < distanceThreshold &&
                                        Math.Abs(distBetween2 - side2.TotalLength) < distanceThreshold)
                                    {
                                        found = true;
                                        rect = new ImageRectangle(new MergedLines[] { side1, side2, other1, other2 });
                                        break;
                                    }
                                }
                            }
                        }
                    }
                }
            }

            return found;
        }

        /// <summary>
        /// Find closest point to a target from a set of start / end points of 
        /// a set of lines
        /// </summary>
        /// <param name="point">Target point</param>
        /// <param name="lines">Set of edges</param>
        /// <returns>Closest point</returns>
        private static Point2D<double> GetClosestPoint(Point2D<double> point, List<MergedLines> lines)
        {
            Point2D<double> close = lines[0].Start;
            double minDistance = MergedLines.LineEndDistance(point.X, point.Y, lines[0].Start.X, lines[0].Start.Y);

            foreach (MergedLines l in lines)
            {
                Point2D<double> cand = l.Start;
                double distance = MergedLines.LineEndDistance(point.X, point.Y, cand.X, cand.Y);

                if (distance < minDistance)
                {
                    close = cand;
                    minDistance = distance;
                }

                cand = l.End;
                distance = MergedLines.LineEndDistance(point.X, point.Y, cand.X, cand.Y);

                if (distance < minDistance)
                {
                    close = cand;
                    minDistance = distance;
                }
            }

            return close;
        }

        /// <summary>
        /// Return the average of a list of angle. Angles in the list will be reflected
        /// about 180 degrees as necessary when averaging
        /// </summary>
        /// <param name="angles">List of angles in degrees</param>
        /// <returns>Average reflected about 180 as necessary</returns>
        private static double AverageAngleWithReflection(params double[] angles)
        {
            double avg = angles[0];
            double count = 1;

            for (int i = 1; i < angles.Length; ++i)
            {
                double angle = angles[i];
                ++count;
                angle = Math.Abs(avg - angle) < Math.Abs(avg - angle - 180) ? angle : angle - 180;
                avg = (count - 1) / count * avg + angle / count;
            }

            return avg;
        }

        /// <summary>
        /// Debug reports  found rectangles
        /// </summary>
        /// <param name="rects">List of rectangles</param>
        private static void ReportFoundRectangle(List<ImageRectangle> rects)
        {
            Console.WriteLine("Found {0} Rects", rects.Count);
            foreach (ImageRectangle r in rects)
            {
                TraceOut.Info(ImageRectangle.rectTraceContext, "Center {0:F1} {1:F1} Lengths {2:F1} {3:F1}  Angles {4:F1} {5:F1}", r.CenterX, r.CenterY, r.MajorLength, r.MinorLength, r.MajorSideAngle, r.MinorSideAngle);
                foreach (Point2D<int> corner in r.GetCorners())
                {
                    TraceOut.Info(ImageRectangle.rectTraceContext, "Corner {0}\t{1}\t", corner.X, corner.Y);
                }
            }
        }
    }
}
